#include <ros/ros.h>
#include <ros/package.h>

#include <message_filters/subscriber.h>
#include <message_filters/time_synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>

#include <sensor_msgs/PointCloud2.h>

#include <pcl/filters/extract_indices.h>

#include <human_tracking/BodyArray.h>
#include <human_tracking/IndicesArray.h>
#include <human_tracking/DataSet.h>

#include <jsk_pcl_ros/color_filter.h>

#include <pcl/segmentation/extract_clusters.h>

class HandBodyExtractor {
  ros::NodeHandle nh_;
  ros::NodeHandle pnh_;

  message_filters::Subscriber<sensor_msgs::PointCloud2> sub_cloud_;
  message_filters::Subscriber<human_tracking::BodyArray> sub_body_;
  message_filters::Subscriber<human_tracking::IndicesArray> sub_indices_;

  boost::shared_ptr<message_filters::Synchronizer<message_filters::sync_policies::ApproximateTime<sensor_msgs::PointCloud2, human_tracking::BodyArray, human_tracking::IndicesArray> > > sync_inputs_a_;

  int max_queue_size_;

  ros::Publisher pub_data_;
public:
  HandBodyExtractor () : nh_(), pnh_("~") {
    pnh_.param("max_queue_size", max_queue_size_, 10);

    sync_inputs_a_ = boost::make_shared <message_filters::Synchronizer<message_filters::sync_policies::ApproximateTime<sensor_msgs::PointCloud2, human_tracking::BodyArray, human_tracking::IndicesArray> > > (max_queue_size_);

    sub_cloud_.subscribe(pnh_, "input", max_queue_size_);
    sub_body_.subscribe(pnh_, "bodies", max_queue_size_);
    sub_indices_.subscribe(pnh_, "indices", max_queue_size_);

    sync_inputs_a_->connectInput (sub_cloud_, sub_body_, sub_indices_);
    sync_inputs_a_->registerCallback (bind (&HandBodyExtractor::callback_sync, this, _1, _2, _3));

    pub_data_ = pnh_.advertise<human_tracking::DataSet> ("data", 10);
  }

  void callback_sync(const sensor_msgs::PointCloud2ConstPtr& pcloud2,
                     const human_tracking::BodyArrayConstPtr &body_array,
                     const human_tracking::IndicesArrayConstPtr &idx_array) {
    ROS_INFO("callback");

    // check size
    int size = idx_array->array.size();
    if ( size != body_array->array.size() ) {
      ROS_WARN("invalid size");
      return;
    }

#if 0
    // extract points
    std::vector < sensor_msgs::PointCloud2 > plst;
    pcl::ExtractIndices< sensor_msgs::PointCloud2 > pei;
    pei.setInputCloud(pcloud2);

    for(int i = 0; i < size; i++) {
      sensor_msgs::PointCloud2 tmp;
      pei.setIndices(boost::make_shared<pcl::PointIndices > (idx_array->array[i]));
      pei.filter(tmp);
      plst.push_back(tmp);
    }

    // skin color extraction
    //pcl::HSVColorFilter<sensor_msgs::PointCloud2> hcf;
    //hcf.setInputCloud(pcloud2);
    //sensor_msgs::PointCloud2 filtered;
    //hcf.filter(filtered);
    {

    }

    // hand extraction
    {
      pcl::EuclideanClusterExtraction< sensor_msgs::PointCloud2 > eu_ext;
    }
#endif

    human_tracking::DataSet data;
    data.header = pcloud2->header;
    data.points = *pcloud2;
    data.body_array = *body_array;
    data.indices_array = *idx_array;
    // hand -> handpos or

    pub_data_.publish(data);
  }
};

int main(int argc, char **argv) {
  ros::init(argc, argv, "hand_body_extractor");

  HandBodyExtractor h;
  ros::spin();

  return -1;
}
